/**
*  \file control_pump.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "control_pump.h"
#include "../app/app.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../cfg/control_pump_cfg.h"

#include "../interpol/interpol_s16_s16.h"
#include "../interpol/interpol_u16_u16.h"
#include "../mlib/max.h"
#include "../supervisor/supervisor.h"
#include "../output/output.h"
#include <stdlib.h>
#include "../mlib/middle.h"
#include "../lithium_ion/li_ion.h"

//#ifdef UNIT_TEST
//	#include "../control_pump_unittest/control_pump_unittest.h"
//	#define static
//#else

/*===============================================[ private defines   ]================================================*/

#define LIFT_POT_CURVE_X_0		0
#define LIFT_POT_CURVE_X_1		1
#define LIFT_POT_CURVE_X_2		2
#define LIFT_POT_CURVE_X_3		3
#define LIFT_POT_CURVE_X_4		4

#define LIFT_POT_CURVE_Y_0		5
#define LIFT_POT_CURVE_Y_1		6
#define LIFT_POT_CURVE_Y_2		7
#define LIFT_POT_CURVE_Y_3		8
#define LIFT_POT_CURVE_Y_4		9

#define SWITCH_FILT				5
#define LIFT_SWITCH_1_ON		((BO)(pump.input.lift_switch_1 > SWITCH_FILT))
#define LIFT_SWITCH_2_ON		((BO)(pump.input.lift_switch_2 > SWITCH_FILT))
#define TILT_SWITCH_ON			((BO)(pump.input.tilt_switch > SWITCH_FILT))
#define AUX1_SWITCH_ON			((BO)(pump.input.aux_switch_1 > SWITCH_FILT))
#define AUX2_SWITCH_ON			((BO)(pump.input.aux_switch_2 > SWITCH_FILT))
#define SEAT_SWITCH_ON			((BO)(pump.input.seat_switch_1 > SWITCH_FILT))

#define STEER_MOTOR_TORQUE_THRESHOLD	6		// N.m
#define TRUCK_SPEED_THRESHOLD			40		// m/h
#define STEER_PRIMARY_RPM				400		// RPM
#define PUMP_MAX_RPM					3000	// RPM
#define PUMP_MIN_RPM					400		// RPM

#define PUMP_SOC_CUTBACK_START			20		// %
#define PUMP_RPM_SOC_CUTBACK			30		// %
#define MOTOR_TEMP_CUT_START			110		// ℃
#define MOTOR_TEMP_CUT_END				140		// ℃

#define CHIP_TEMP_CUT_START				75		// ℃
#define CHIP_TEMP_CUT_END				90		// ℃

#define TILT_AUX_CHIP_TEMP_CUT			100		//℃

#define MOTOR_DISABLE					1
#define MOTOR_ENABLE					2

#define PWM_DISABLE						0
#define PWM_ENABLE						1


/*===============================================[ private datatypes ]================================================*/

typedef struct PUMP_INPUT
{
	S16 lift_switch_1;
	S16 lift_switch_2;
	S16 lift_potentio_ratio;
	S16 tilt_switch;
	S16 aux_switch_1;
	S16 aux_switch_2;
	S16 seat_switch_1;
	S16 SOC;

	S16 acc_pedal;
	S16 truck_speed;		//m/h
	S16 trac_motor_current;

	BO magnet_ready;
	F32 motor_torque;			//N.m
	S16 motor_measured_rpm;		//measured motor RPM;

	S16 motor_temperature;		//℃
	S16 chip_temperature;		//℃
}PUMP_INPUT;

typedef enum LIFT_MODE
{
	invalid				=	0,
	one_lift_switch		= 	1,
	two_lift_switch		=	2,
	potentiometer		=	3
}LIFT_MODE;

typedef struct PUMP_PARAMETER
{
	S16 max_rpm;							//unused
	S16 steer_truck_speed_threshold;		//threshold to start pump for steering, m/h, used now
	S16 steer_primary_rpm;					//low speed when torque is small
	S16 steer_secondary_rpm;				//high speed when torque is big
	S16 steer_motor_torque_threshold;		//torque threshold to switch pump motor rpm, unused now
	S16 steer_close_delay;
	S16 steer_ramp_up;
	S16 steer_ramp_down;

	LIFT_MODE lift_mode;
	S16 lift_1s_rpm;
	S16 lift_2s_rpm_1;
	S16 lift_2s_rpm_2;
	S16 lift_pot_min_prm;
	S16 lift_pot_max_rpm;
	S16 lift_ramp_up;
	S16 lift_ramp_down;

	S16 tilt_rpm;
	S16 tilt_ramp_up;
	S16 tilt_ramp_down;

	S16 aux1_rpm;
	S16 aux1_ramp_up;
	S16 aux1_ramp_down;

	S16 aux2_rpm;
	S16 aux2_ramp_up;
	S16 aux2_ramp_down;

	U8 soc_cutback_start;		// unused
	U8 soc_cutback_end;			// unused
	U8 soc_rpm_cutback;			// unused

	U8	motor_discharge_enable;
	S16 regenration_current_threshold;
	S16 soc_threshold;		//when soc > this threshold, and regeneration current > setting current, rotate pump with full speed to discharge
    U8 motor_temp_red_start;
    U8 motor_temp_red_end;
    U8 motor_temp_rpm_cutback;
    U8 chip_temp_red_start;
    U8 chip_temp_red_end;
    U8 chip_temp_rpm_cutback;
}PUMP_PARAMETER;

typedef struct PUMP_OUTPUT
{
	S16 final_motor_rpm;
	S16 motor_torque;
}PUMP_OUTPUT;

typedef struct PUMP
{
	PUMP_INPUT 		input;
	PUMP_PARAMETER	parameter;
	PUMP_OUTPUT		output;
}PUMP;

typedef struct  PUMP_FALG
{
    BO   steerFlag;
	BO   liftFlag;
	BO   tiltFlag;
	BO   aux1Flag;
	BO   aux2Flag;
}PUMP_FLAG;

//#endif
/*===============================================[ private variables ]================================================*/

static enum trilean disable_pump;

static PUMP_FLAG pumpF;
static F32 steer_current_rpm;
static F32 lift_current_rpm;
static F32 tilt_current_rpm;
static F32 aux1_current_rpm;
static F32 aux2_current_rpm;

static S16 steer_target_rpm;
static S16 lift_target_rpm;
static S16 tilt_target_rpm;
static S16 aux1_target_rpm;
static S16 aux2_target_rpm;

static S16 rpm_output_buffer;

static S16 lift_pot_curve_x[5];
static S16 lift_pot_curve_y[5];
static struct interpol_s16_s16_obj lift_potentio_curve;

static S16 cnt_1st_rpm_delay;
static S16 cnt_2nd_rpm_delay;

static S16 primary_target_rpm;
static S16 secondary_target_rpm;

static F32 step;
static S16 cnt2s;

static BO SOC_reduction_flag;

static S16 lift_max_rpm;
static S16 motor_temp_red_lift_rpm;
static S16 chip_temp_red_lift_rpm;
static S16 chip_temp_red_tilt_rpm;
static S16 chip_temp_red_aux1_rpm;
static S16 chip_temp_red_aux2_rpm;

static BO pump_motor_discharge;

static S16 cnt_truck_moving;
static BO truck_moving_flag;

PUMP pump;

/*===============================================[ public variables  ]================================================*/

S16 pump_motor_enable;			//enable the converter
S16 pump_motor_PWM_enable;		//enable the PWM (if OK, the magnetReady will be 1);

/*===============================================[ private functions ]================================================*/

static void control_pump_input_update();

static void control_pump_parameter_update();

static void pump_parameter_rx(U16 RxMsgData[4]);

static void pump_parameter_tx(U16 TxMsgData[4]);

static void control_pump_steer();

static void control_pump_lift();

static void control_pump_tilt();

static void control_pump_aux1();

static void control_pump_aux2();

static void ramp_control(S16 target_value, F32* current_value, S16 ramp_y, S16 ramp_up_x, S16 ramp_down_x);

static void output_max_rpm();

static void interpol_s16_s16_construct( struct interpol_s16_s16_obj *me, pal_size_t count, S16 const * p_x, S16 const * p_y );

static void SOC_reduction_control();

static void motor_temp_reduction_control();

static void chip_temp_reduction_control();

static void pump_motor_discharge_control();

static void pump_motor_enable_control();

static void truck_moving_filter();

/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC control_pump_initialize(void)
{
	pal_memzero(&pump, sizeof(pump));
	pal_memzero(&pumpF, sizeof( pumpF));


	pal_memzero(lift_pot_curve_x, sizeof(lift_pot_curve_x));
	pal_memzero(lift_pot_curve_y, sizeof(lift_pot_curve_y));
	pal_memzero(&lift_potentio_curve, sizeof(lift_potentio_curve));

	disable_pump 		= 	trilean_true;
	steer_current_rpm	=	0;
	lift_current_rpm	=	0;
	tilt_current_rpm	=	0;
	aux1_current_rpm	=	0;
	aux2_current_rpm	=	0;

	steer_target_rpm	=	0;
	lift_target_rpm		=	0;
	tilt_target_rpm		=	0;
	aux1_target_rpm		=	0;
	aux2_target_rpm		=	0;

	pump_motor_enable		=	0;
	pump_motor_PWM_enable	=	0;

	cnt_1st_rpm_delay = 0;
	cnt_2nd_rpm_delay = 0;
	primary_target_rpm = 0;
	secondary_target_rpm = 0;

	step = 0;
	cnt2s = 0;

	SOC_reduction_flag = FALSE;
	lift_max_rpm = 0;
	motor_temp_red_lift_rpm = 0;
	chip_temp_red_lift_rpm = 0;
	chip_temp_red_tilt_rpm = 0;
	chip_temp_red_aux1_rpm = 0;
	chip_temp_red_aux2_rpm = 0;

	pump_motor_discharge = 0;

	cnt_truck_moving = 0;
	truck_moving_flag = FALSE;

	interpol_s16_s16_construct(&lift_potentio_curve, 5, lift_pot_curve_x, lift_pot_curve_y);

	return RC_SUCCESS;
}

S16 control_pump_10ms()
{
	control_pump_input_update();
	control_pump_parameter_update();
	disable_pump = fDisPump;

	control_pump_steer();
	control_pump_lift();
	control_pump_tilt();
	control_pump_aux1();
	control_pump_aux2();

	motor_temp_reduction_control();	//lift RPM reduction according motor temperature.

	chip_temp_reduction_control();		// RPM reduction according chip temperature.

	output_max_rpm();

	SOC_reduction_control();

	pump_motor_discharge_control();

	pump_motor_enable_control();

	return pump.output.final_motor_rpm;
}

S16 calculate_pump_torque_10ms()
{
	pump.output.motor_torque = 0;

	return pump.output.motor_torque;
}

/*===============================================[ private functions ]================================================*/

static void control_pump_input_update()
{
	pump.input.acc_pedal					=	input_data.accelerator;
	pump.input.truck_speed					=	output_data.te_vSen * 1000;		//te_vSen unit: Km/h

	pump.input.lift_switch_1				=	input_data.lift_switch_1;
	pump.input.lift_switch_2				=	input_data.lift_switch_2;
	pump.input.lift_potentio_ratio			=	input_data.lift_potentiometer_ratio;
	pump.input.tilt_switch					=	input_data.tilt_switch;
	pump.input.aux_switch_1					=	input_data.hyd_aux_switch_1;
	pump.input.aux_switch_2					=	input_data.hyd_aux_switch_2;
	pump.input.seat_switch_1				=	input_data.seat_switch_1;
	pump.input.SOC							=	input_data.bms_soc_with_soh;
	pump.input.trac_motor_current			=	(S16)input_data.mL_iBat_Can;		//traction motor battery current (line current)
	pump.input.magnet_ready					=	input_data.mE_StsMagnetized_Can;
	pump.input.motor_torque					=	input_data.mE_mEval_Can;
	pump.input.motor_measured_rpm			=	(S16)input_data.mE_nSen_Can;
	pump.input.motor_temperature			=	(S16)input_data.mE_TMot_Can;
	pump.input.chip_temperature				=	(S16)input_data.mE_TMos_Can;
}

static void control_pump_parameter_update()
{
	pump.parameter.max_rpm							=	service->pump_max_rpm;
	pump.parameter.steer_truck_speed_threshold		=	service->pump_truck_speed_threshold;		// m/h		//threshold to start pump for steering.
	pump.parameter.steer_primary_rpm				=	service->pump_steer_primary_speed;
	pump.parameter.steer_secondary_rpm				=	service->pump_steer_secondary_speed;
	pump.parameter.steer_motor_torque_threshold		=	service->pump_steer_torque_threshold;
	pump.parameter.steer_close_delay				=	service->pump_steer_close_delay;		//*10ms
	pump.parameter.steer_ramp_up					=	service->pump_steer_ramp_up;			//*10ms
	pump.parameter.steer_ramp_down					=	service->pump_steer_ramp_down;			//*10ms

	pump.parameter.lift_mode						=	service->option_lift;
	pump.parameter.lift_1s_rpm						=	service->pump_lift_1s_rpm;
	pump.parameter.lift_2s_rpm_1					=	service->pump_lift_2s_rpm_1;
	pump.parameter.lift_2s_rpm_2					=	service->pump_lift_2s_rpm_2;
	pump.parameter.lift_pot_min_prm					=	service->pump_lift_pot_min_rpm;
	pump.parameter.lift_pot_max_rpm					=	service->pump_lift_pot_max_rpm;
	pump.parameter.lift_ramp_up						=	service->pump_lift_ramp_up;
	pump.parameter.lift_ramp_down					=	service->pump_lift_ramp_down;

	pump.parameter.tilt_rpm							=	service->pump_tilt_rpm;
	pump.parameter.tilt_ramp_up						=	service->pump_tilt_ramp_up;
	pump.parameter.tilt_ramp_down					=	service->pump_tilt_ramp_down;

	pump.parameter.aux1_rpm							=	service->pump_aux1_rpm;
	pump.parameter.aux1_ramp_up						=	service->pump_aux1_ramp_up;
	pump.parameter.aux1_ramp_down					=	service->pump_aux1_ramp_down;

	pump.parameter.aux2_rpm							=	service->pump_aux2_rpm;
	pump.parameter.aux2_ramp_up						=	service->pump_aux2_ramp_up;
	pump.parameter.aux2_ramp_down					=	service->pump_aux2_ramp_down;

	pump.parameter.soc_cutback_start				=	service->pump_soc_reduction_start;
	pump.parameter.soc_cutback_end					=	service->pump_soc_reduction_end;
	pump.parameter.soc_rpm_cutback					=	service->pump_soc_rpm_cutback;

	pump.parameter.motor_discharge_enable			=	service->option_pump_motor_discharge_enable;
	pump.parameter.regenration_current_threshold	=	service->pump_discharge_regen_current_thd;
	pump.parameter.soc_threshold					=	service->pump_discharge_soc_thd;

    pump.parameter.motor_temp_red_start				=	service->pump_motor_temp_red_end;
    pump.parameter.motor_temp_red_end				=	service->pump_motor_temp_red_end;
    pump.parameter.motor_temp_rpm_cutback			=	service->pump_motor_temp_rpm_cutback;

    pump.parameter.chip_temp_red_start				=	service->pump_chip_temp_red_start;
    pump.parameter.chip_temp_red_end				=	service->pump_chip_temp_red_end;
    pump.parameter.chip_temp_rpm_cutback			=	service->pump_chip_temp_rpm_cutback;

	lift_pot_curve_x[0]								=	0;
	lift_pot_curve_x[1]								=	1;
	lift_pot_curve_x[2]								=	60;
	lift_pot_curve_x[3]								=	95;
	lift_pot_curve_x[4]								=	100;

	lift_pot_curve_y[0]								=	0;
	lift_pot_curve_y[1]								=	pump.parameter.lift_pot_min_prm;
	lift_pot_curve_y[2]								=	(pump.parameter.lift_pot_min_prm + pump.parameter.lift_pot_max_rpm) / 2;
	lift_pot_curve_y[3]								=	pump.parameter.lift_pot_max_rpm;
	lift_pot_curve_y[4]								=	pump.parameter.lift_pot_max_rpm;
	
	pump_parameter_rx(input_data.v_debug_rx_0);
	pump_parameter_tx(output_data.v_debug_4);
}

static void pump_parameter_rx(U16 RxMsgData[4])
{
	U8 ModuleID;
	U8 ParamID;
	
	ModuleID = (U8)RxMsgData[0];
	ParamID = (U8)(RxMsgData[0]>>8);

	if(ModuleID != 0x02u)
	{
	}
	else
	{
		switch (ParamID)
		{
		case LIFT_POT_CURVE_X_0:
			lift_pot_curve_x[0] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_1:
			lift_pot_curve_x[1] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_2:
			lift_pot_curve_x[2] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_3:
			lift_pot_curve_x[3] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_4:
			lift_pot_curve_x[4] = RxMsgData[1];
			break;

		case LIFT_POT_CURVE_Y_0:
			lift_pot_curve_y[0] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_1:
			lift_pot_curve_y[1] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_2:
			lift_pot_curve_y[2] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_3:
			lift_pot_curve_y[3] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_4:
			lift_pot_curve_y[4] = RxMsgData[1];
			break;
		default:
			break;
		}
	}
}

static void pump_parameter_tx(U16 TxMsgData[4])
{
	static U16 ParaID = 0;

	TxMsgData[0] = 0;
	TxMsgData[0] |= 0x02;
	TxMsgData[0] |= (ParaID<<8);

	switch (ParaID)
	{
	case LIFT_POT_CURVE_X_0:
		TxMsgData[1] = lift_pot_curve_x[0];
		break;
	case LIFT_POT_CURVE_X_1:
		TxMsgData[1] = lift_pot_curve_x[1];
		break;
	case LIFT_POT_CURVE_X_2:
		TxMsgData[1] = lift_pot_curve_x[2];
		break;
	case LIFT_POT_CURVE_X_3:
		TxMsgData[1] = lift_pot_curve_x[3];
		break;

	case LIFT_POT_CURVE_Y_0:
		TxMsgData[1] = lift_pot_curve_y[0];
		break;
	case LIFT_POT_CURVE_Y_1:
		TxMsgData[1] = lift_pot_curve_y[1];
		break;
	case LIFT_POT_CURVE_Y_2:
		TxMsgData[1] = lift_pot_curve_y[2];
		break;
	case LIFT_POT_CURVE_Y_3:
		TxMsgData[1] = lift_pot_curve_y[3];
		break;

	default:
		break;
	}

	ParaID++;
	if(ParaID>7u)
	{
		ParaID = 0;
	}

	TxMsgData[2] = 0;
	TxMsgData[3] = 0;
}

/*
 * function: to avoid truck short-time movement to trigger steering function
 * stopping lowering weight may causes short-time truck movement, which may trigger steer function.
 * input:	pump.input.truck_speed
 * output:	truck_moving_flag
 */
static void truck_moving_filter()
{
	if(abs(pump.input.truck_speed) >= TRUCK_SPEED_THRESHOLD)
	{
		cnt_truck_moving ++;
	}
	else
	{
		cnt_truck_moving --;
	}
	cnt_truck_moving = middle_s16(0, cnt_truck_moving, 50);
	if(cnt_truck_moving >= 50)		// truck moving for 500ms, to avoid to trigger steering by sudden truck moving for example caused by lowering weight
	{
		truck_moving_flag = TRUE;
	}
	else if(cnt_truck_moving <= 0)
	{
		truck_moving_flag = FALSE;
	}
	else
	{
		// do nothing;
	}
}

static void control_pump_steer()
{
	static BO steer_enable = FALSE;

	static S16 cnt_seat_on = 0;
	static S16 cnt_steer_recovery = 0;

	BO trigger_steer_flag1 = FALSE;
	BO trigger_steer_flag2 = FALSE;

	BO tmp_cnt_steer_recovery = FALSE;

	if(trilean_true == disable_pump)
	{
		steer_enable = FALSE;
		primary_target_rpm = 0;
		secondary_target_rpm = 0;
		steer_target_rpm = 0;
		steer_current_rpm = 0;
		pumpF.steerFlag = FALSE;

		cnt_seat_on = 0;
		cnt_truck_moving = 0;
	}
	else
	{
		if(SEAT_SWITCH_ON)
		{
			steer_enable = TRUE;
			cnt_seat_on = 50;		//delay for 500ms if seat switch off
		}
		else
		{
			if(cnt_seat_on > 0)
			{
				cnt_seat_on--;
			}
			else
			{
				if(abs(pump.input.truck_speed) < 2000)
				{
					steer_enable = FALSE;
				}
				else
				{
					tmp_cnt_steer_recovery = TRUE;

					if(LIFT_SWITCH_1_ON || (pump.input.lift_potentio_ratio > 0) || TILT_SWITCH_ON || AUX1_SWITCH_ON || AUX2_SWITCH_ON)
					{
						steer_enable = FALSE;
						steer_current_rpm = 0;		//stop steer RPM immediately
						cnt_steer_recovery = 20;	//delay for 200ms before recover steer
					}
					else
					{
						if(cnt_steer_recovery > 0)
						{
							cnt_steer_recovery--;
						}
						else
						{
							steer_enable = TRUE;
						}
					}
				}
			}
		}

		truck_moving_filter();

		if(steer_enable)
		{
			/*to realize primary RPM*/
			if((pump.input.acc_pedal > 0) || truck_moving_flag)
			{
				pumpF.steerFlag = TRUE;
				primary_target_rpm = STEER_PRIMARY_RPM;
				cnt_1st_rpm_delay = pump.parameter.steer_close_delay;		//delay once steer is running;
			}
			else
			{
				if(cnt_1st_rpm_delay > 0)
				{
					cnt_1st_rpm_delay--;
				}
				else
				{
					primary_target_rpm = 0;
				}
			}

			if(pumpF.liftFlag || pumpF.tiltFlag || pumpF.aux1Flag || pumpF.aux2Flag)
			{
				/*once hydraulic functions activating */
				if((lift_target_rpm == 0) && (tilt_target_rpm == 0) && (aux1_target_rpm == 0) && (aux2_target_rpm == 0))
				{
					/*during this period, hydraulic lever is released */
					if((pump.input.motor_measured_rpm <= PUMP_MIN_RPM) && (pump.input.motor_torque > STEER_MOTOR_TORQUE_THRESHOLD))
					{
						/*in this conditions, no matter whether it`s steering, start pump for steering*/
						pumpF.steerFlag = TRUE;
						trigger_steer_flag2 = TRUE;
					}
					else
					{
						/*do nothing*/
					}
				}
				else
				{
					/*do nothing*/
				}
			}
			else
			{
				if(pumpF.steerFlag && (pump.input.motor_torque > STEER_MOTOR_TORQUE_THRESHOLD))
				{
					trigger_steer_flag1 = TRUE;
				}
				else
				{
					/*do nothing*/
				}
			}

			if(trigger_steer_flag1 || trigger_steer_flag2)
			{
				secondary_target_rpm = pump.parameter.steer_secondary_rpm;
				cnt_2nd_rpm_delay = pump.parameter.steer_close_delay;
			}
			else
			{
				if(cnt_2nd_rpm_delay > 0)
				{
					cnt_2nd_rpm_delay--;
				}
				else
				{
					secondary_target_rpm = 0;
				}
			}
		}
		else		//steer not enable;
		{
			primary_target_rpm = 0;
			secondary_target_rpm = 0;

			cnt_1st_rpm_delay = 0;
			cnt_2nd_rpm_delay = 0;
		}

		steer_target_rpm = max2_s16(primary_target_rpm, secondary_target_rpm);

		if(steer_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			steer_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(steer_target_rpm, &steer_current_rpm, pump.parameter.steer_primary_rpm, pump.parameter.steer_ramp_up, pump.parameter.steer_ramp_down);
		}
		if(((S16)steer_current_rpm == 0) && (steer_target_rpm == 0))
		{
			pumpF.steerFlag = FALSE;
		}
	}

	if(!tmp_cnt_steer_recovery)		//if delay condition is not met, delay is set at initial value
	{
		cnt_steer_recovery = 0;
	}
}

static void control_pump_lift()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))		// > 5 means switch is engaged
	{
		lift_target_rpm = 0;
		lift_current_rpm = 0;
		pumpF.liftFlag = FALSE;
	}
	else
	{
		/*3 lift control modes*/
		switch(pump.parameter.lift_mode)
		{
		case invalid:
			// lift mode not set, do nothing;
			lift_max_rpm = 0;
			break;

		case one_lift_switch:
			if(TRUE == LIFT_SWITCH_1_ON)		//lift level is operated
			{
				pumpF.liftFlag = TRUE;
				lift_target_rpm = pump.parameter.lift_1s_rpm;
			}
			else
			{
				lift_target_rpm = 0;
			}

			lift_max_rpm = pump.parameter.lift_1s_rpm;
			break;

		case two_lift_switch:
			if(TRUE == LIFT_SWITCH_1_ON)		// lift switch 1 is operated
			{
				pumpF.liftFlag = TRUE;

				if(TRUE == LIFT_SWITCH_2_ON)		//lift switch 2 is operated
				{
					lift_target_rpm = pump.parameter.lift_2s_rpm_2;
				}
				else
				{
					lift_target_rpm = pump.parameter.lift_2s_rpm_1;
				}
			}
			else
			{
				lift_target_rpm = 0;
			}

			lift_max_rpm = pump.parameter.lift_2s_rpm_2;
			break;

		case potentiometer:
			lift_target_rpm = (S32)interpol_s16_s16(&lift_potentio_curve, pump.input.lift_potentio_ratio);
			if(lift_target_rpm > 0)
			{
				pumpF.liftFlag = TRUE;
			}

			lift_max_rpm = pump.parameter.lift_pot_max_rpm;
			break;

		default:
			break;
		}

		lift_target_rpm = middle_s16(0, lift_target_rpm, motor_temp_red_lift_rpm);	//reduce RPM by motor temperature;
		lift_target_rpm = middle_s16(0, lift_target_rpm, chip_temp_red_lift_rpm);	//reduce RPM by chip temperature;

		if(pump.parameter.motor_discharge_enable && pump_motor_discharge)		// pump motor discharge control when SOC and traction motor regen current is big.
		{
			lift_target_rpm = lift_max_rpm;
		}
		else
		{
			// do nothing;
		}

		if(lift_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			lift_current_rpm = pump.output.final_motor_rpm;
		}

		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(lift_target_rpm, &lift_current_rpm, lift_max_rpm, pump.parameter.lift_ramp_up, pump.parameter.lift_ramp_down);
		}

		if(((S16)lift_current_rpm == 0) && (lift_target_rpm == 0))
		{
			pumpF.liftFlag = FALSE;
		}
	}
}

static void control_pump_tilt()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))   //F
	{
		tilt_target_rpm = 0;
		tilt_current_rpm = 0;
		pumpF.tiltFlag = FALSE;
	}
	else
	{
		if(TRUE == TILT_SWITCH_ON)		//F
		{
			pumpF.tiltFlag = TRUE;
			tilt_target_rpm = pump.parameter.tilt_rpm;
		}
		else
		{
			tilt_target_rpm = 0;
		}

		tilt_target_rpm = middle_s16(0, tilt_target_rpm, chip_temp_red_tilt_rpm);	//reduce rpm according chip temperature;

		if(tilt_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			tilt_current_rpm = pump.output.final_motor_rpm;
		}

		if(TRUE == pump.input.magnet_ready)		//F
		{
			ramp_control(tilt_target_rpm, &tilt_current_rpm, pump.parameter.tilt_rpm, pump.parameter.tilt_ramp_up, pump.parameter.tilt_ramp_down);
		}

		if(((S16)tilt_current_rpm == 0) && (tilt_target_rpm == 0))	//T
		{
			pumpF.tiltFlag = FALSE;
		}
	}
}

static void control_pump_aux1()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))
	{
		aux1_target_rpm = 0;
		aux1_current_rpm = 0;
		pumpF.aux1Flag = FALSE;
	}
	else
	{
		if(TRUE == AUX1_SWITCH_ON)
		{
			pumpF.aux1Flag = TRUE;
			aux1_target_rpm = pump.parameter.aux1_rpm;
		}
		else
		{
			aux1_target_rpm = 0;
		}

		aux1_target_rpm = middle_s16(0, aux1_target_rpm, chip_temp_red_aux1_rpm);	//reduce rpm according chip temperature;

		if(aux1_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			aux1_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(aux1_target_rpm, &aux1_current_rpm, pump.parameter.aux1_rpm, pump.parameter.aux1_ramp_up, pump.parameter.aux1_ramp_down);
		}
		if(((S16)aux1_current_rpm == 0) && (aux1_target_rpm == 0))
		{
			pumpF.aux1Flag = FALSE;
		}
	}
}

static void control_pump_aux2()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))
	{
		aux2_target_rpm = 0;
		aux2_current_rpm = 0;
		pumpF.aux2Flag = FALSE;
	}
	else
	{
		if(TRUE == AUX2_SWITCH_ON)
		{
			pumpF.aux2Flag = TRUE;
			aux2_target_rpm = pump.parameter.aux2_rpm;
		}
		else
		{
			aux2_target_rpm = 0;
		}

		aux2_target_rpm = middle_s16(0, aux2_target_rpm, chip_temp_red_aux2_rpm);	//reduce rpm according chip temperature;

		if(aux2_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			aux2_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(aux2_target_rpm, &aux2_current_rpm, pump.parameter.aux2_rpm, pump.parameter.aux2_ramp_down, pump.parameter.aux2_ramp_down);
		}
		if(((S16)aux2_current_rpm == 0) && (aux2_target_rpm == 0))
		{
			pumpF.aux2Flag = FALSE;
		}
	}
}

/*should be called in 10ms cycle*/
static void ramp_control(S16 target_value, F32* current_value, S16 ramp_y, S16 ramp_up_x, S16 ramp_down_x)
{
	if(*current_value < target_value)		//ramp up
	{
		if(ramp_up_x <= 0)
		{
			*current_value = target_value;
		}
		else
		{
			step = (F32)ramp_y / ramp_up_x;
			*current_value += step;
			if(*current_value >= target_value)
			{
				*current_value = target_value;
			}
		}
	}
	else if(*current_value > target_value)		//ramp down
	{
		if(ramp_down_x <= 0)
		{
			*current_value = target_value;
		}
		else
		{
			step = (F32)ramp_y / ramp_down_x;
			*current_value -= step;
			if(*current_value <= target_value)
			{
				*current_value = target_value;
			}
		}
	}
	else
	{

	}
}

static void output_max_rpm()
{
	F32 rv = 0;
	rv = max4_f32(lift_current_rpm, tilt_current_rpm, aux1_current_rpm, aux2_current_rpm);
	rpm_output_buffer = (S16)max2_f32(rv, steer_current_rpm);
}

static void interpol_s16_s16_construct( struct interpol_s16_s16_obj *me, pal_size_t count, S16 const * p_x, S16 const * p_y )
{
	me->count= count;
	me->x= p_x;
	me->y= p_y;
}

static void SOC_reduction_control()
{
	static U16 SOCLowCnt = 0;

	if(pump.input.SOC < PUMP_SOC_CUTBACK_START)
	{
		if(SOCLowCnt < 6000)	// 1min == 60s == 60 * 100(10ms) = 6000(10ms);
		{
			SOCLowCnt++;
		}
		else	// if low SOC keeps for 1min
		{
			if((!pumpF.liftFlag) && (!pumpF.tiltFlag) && (!pumpF.aux1Flag) && (!pumpF.aux2Flag))	//only when all hydraulic functions stop, low SOC reduction activated
			{
				SOC_reduction_flag = TRUE;
			}
			else
			{
				// do nothing
			}
		}
	}
	else
	{
		if(SOCLowCnt > 0)
		{
			SOCLowCnt--;
		}
		else
		{
			if((!pumpF.liftFlag) && (!pumpF.tiltFlag) && (!pumpF.aux1Flag) && (!pumpF.aux2Flag))	//only when all hydraulic functions stop, low SOC reduction disabled
			{
				SOC_reduction_flag = FALSE;
			}
			else
			{
				// do nothing
			}
		}
	}

	if(SOC_reduction_flag)	//reduce RPM out put according low SOC strategy
	{
		rpm_output_buffer = middle_s16(PUMP_MIN_RPM, rpm_output_buffer, (S32)(PUMP_MAX_RPM) * PUMP_RPM_SOC_CUTBACK / 100);
	}
	else
	{
		// do nothing, just keep 100% RPM as output
	}
}

static void pump_motor_discharge_control()
{
	if(trilean_true == disable_pump)
	{
		rpm_output_buffer = 0;
		pump.output.final_motor_rpm = 0;
	}
	else
	{
		if(FALSE == pump.parameter.motor_discharge_enable)
		{
			// do nothing
		}
		else
		{
			if(((pump.input.SOC >= pump.parameter.soc_threshold) && (pump.input.trac_motor_current <= (-pump.parameter.regenration_current_threshold)))
			|| (IsLithiumIonBattery() && (-GetBatterCurrent() > GetBatterMaxChargeCurrent_2sec()))
			)
			{
				pump_motor_discharge = TRUE;
			}
			else
			{
				pump_motor_discharge = FALSE;
			}
		}

		pump.output.final_motor_rpm = rpm_output_buffer;
	}
}

/*
 * reduce lift max RPM according motor temperature
 * steer, tilt, aux1, aux2 is not required to reduce rpm according motor temperature
 */
static void motor_temp_reduction_control(void)
{
	if(MOTOR_TEMP_CUT_END == MOTOR_TEMP_CUT_START)
	{
		if(pump.input.motor_temperature <= MOTOR_TEMP_CUT_START)
		{
			motor_temp_red_lift_rpm = lift_max_rpm;
		}
		else
		{
			motor_temp_red_lift_rpm = 0;
		}
	}
	else
	{
		if(pump.input.motor_temperature <= MOTOR_TEMP_CUT_START)
		{
			motor_temp_red_lift_rpm = lift_max_rpm;
		}
		else if(pump.input.motor_temperature <= MOTOR_TEMP_CUT_END)
		{
			motor_temp_red_lift_rpm = lift_max_rpm + (S32)(PUMP_MIN_RPM - lift_max_rpm)*(pump.input.motor_temperature - MOTOR_TEMP_CUT_START)/(MOTOR_TEMP_CUT_END - MOTOR_TEMP_CUT_START);
		}
		else
		{
			motor_temp_red_lift_rpm = 0;
		}
	}
}

/*
 * reduce lift, tilt, aux1, aux2 max RPM according chip temperature;
 */
static void chip_temp_reduction_control(void)
{
	if(CHIP_TEMP_CUT_END == CHIP_TEMP_CUT_START)
	{
		if(pump.input.chip_temperature <= CHIP_TEMP_CUT_START)
		{
			chip_temp_red_lift_rpm = lift_max_rpm;
		}
		else
		{
			chip_temp_red_lift_rpm = 0;
		}
	}
	else
	{
		if(pump.input.chip_temperature <= CHIP_TEMP_CUT_START)
		{
			chip_temp_red_lift_rpm = lift_max_rpm;
		}
		else if(pump.input.chip_temperature <= CHIP_TEMP_CUT_END)
		{
			chip_temp_red_lift_rpm = lift_max_rpm + (S32)(PUMP_MIN_RPM - lift_max_rpm)*(pump.input.chip_temperature - CHIP_TEMP_CUT_START)/(CHIP_TEMP_CUT_END - CHIP_TEMP_CUT_START);
		}
		else
		{
			chip_temp_red_lift_rpm = 0;
		}
	}

	/*reduce max rpm for tilt, aux1, aux2*/
	if(pump.input.chip_temperature <= TILT_AUX_CHIP_TEMP_CUT)
	{
		chip_temp_red_tilt_rpm = pump.parameter.tilt_rpm;
		chip_temp_red_aux1_rpm = pump.parameter.aux1_rpm;
		chip_temp_red_aux2_rpm = pump.parameter.aux2_rpm;
	}
	else
	{
		chip_temp_red_tilt_rpm = 0;
		chip_temp_red_aux1_rpm = 0;
		chip_temp_red_aux2_rpm = 0;
	}
}
static void pump_motor_enable_control()
{
	if(trilean_true == disable_pump)
	{
		pump_motor_enable = MOTOR_DISABLE;		//motor converter disable;
		pump_motor_PWM_enable = 0;	//motor magnetizing disable;
		cnt2s = 0;
	}
	else
	{
		pump_motor_enable = MOTOR_ENABLE;		//motor converter enable

		if(pumpF.steerFlag || pumpF.liftFlag || pumpF.tiltFlag || pumpF.aux1Flag || pumpF.aux2Flag)
		{
			pump_motor_PWM_enable = PWM_ENABLE;		//magnetizing enable;
			cnt2s = 200;					//keep magnetizing for another 2s
		}
		else
		{
			if(cnt2s>0)
			{
				cnt2s--;
			}
			else
			{
				pump_motor_PWM_enable = PWM_DISABLE;
			}
		}
	}
}

/***********************************************************************************************************************
*                             Copyright 2020 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/












